% calculate the outputs for AMBER model
% outputs include: 1. hip position
%                  2. linearized hip position
%                  3. non-stance slope
%                  4. linearized non-stance slope
%                  5. stance COM slope
%                  6. linearized stance COM slope
function outputs = cal_robot_para_18(q,model_name)

if strcmp(model_name,'AMBER')
addpath('./models/amber/build_ntorso');
elseif strcmp(model_name,'NAO')
    addpath('./models/NAO/build_ntorso');
elseif strcmp(model_name,'HM')
    addpath('./models/human/build_ntorso')
end

for i=1:size(q,2)
    % 1. hip position
    hippos(i) = hipposFn(q(:,i));
    % 2. linearized hip position
    delta_hippos(i) = deltaHipposFn(q(:,i));
    % 3. nonstance slope
    nsslope(i) = nsslopeFn(q(:,i));
    % 4. linearized non-stance slope
    delta_nsslope(i) = deltaNSslopeFn(q(:,i));
    % torso related angle
    % 5. stance COM slope
    stCOM(i) = stCOMFn(q(:,i));
    % 6. linearized stance COM slope
    delta_stCOM(i) = deltastCOMFn(q(:,i));
    % 7. non-stance COM slope
    nstCOM(i) = nstCOMFn(q(:,i));
    % 8. linearized non-stance COM slope
    delta_nstCOM(i) = deltanstCOMFn(q(:,i));
    % 9. COM horizontal position
    COMpos(i) = COMFn(q(:,i));
    % 10. linearized COM horizontal position
    delta_COMpos(i) = deltaCOMFn(q(:,i));
    % 11. stance length of COM
    stCOMLen(i) = stCOMLenFn(q(:,i));
    % 12. non-stance length of COM
    nstCOMLen(i) = nstCOMLenFn(q(:,i));
    % 11. stance length of Hip
    stHipLen(i) = stHipLenFn(q(:,i));
    % 12. non-stance length of Hip
    nstHipLen(i) = nstHipLenFn(q(:,i));
    % 13. COM Angle
    COMAngle(i) = COMAngleFn(q(:,i)); 
    % 14 virtual hip angle
    HipAngleV(i) = HipAngleFn(q(:,i));
end
outputs.hippos = hippos-hippos(1);
outputs.delta_hippos = delta_hippos-delta_hippos(1);
outputs.nsslope = nsslope;
outputs.delta_nsslope = delta_nsslope;
outputs.stCOM = stCOM-stCOM(1);
% plot(COMpos);hold on;
outputs.delta_stCOM = delta_stCOM-delta_stCOM(1);
outputs.nstCOM = nstCOM;
% plot(nstCOM); hold on;
outputs.delta_nstCOM = delta_nstCOM;
outputs.COMpos = COMpos-COMpos(1);
outputs.delta_COMpos = delta_COMpos-delta_COMpos(1);
outputs.stCOMLen = stCOMLen;
% plot(stCOMLen);hold on;
outputs.nstCOMLen = nstCOMLen;
outputs.stHipLen = stHipLen;
outputs.nstHipLen = nstHipLen;
outputs.COMangle = COMAngle;
outputs.HipV = HipAngleV;
% plot(HipAngleV); hold on;
end